/*
kalman.c
offer kalman filter function
designed by lunanting

-----------------------
2020-07-28 20:19:25
create file
-----------------------
*/
#include "stdint.h"
#include "stdlib.h"
#include "kalman.h"
#include "string.h"



/*
kalman filter init
param:
    R:过程噪音
    Q:测量噪音
return:kalman object
*/
KALMAN_class *xp_kalman_init(double R,double Q){
    KALMAN_class *obj;
    obj=(KALMAN_class*)malloc(sizeof(KALMAN_class));
    memset(obj,0,sizeof(KALMAN_class));
    obj->Q=Q;
    obj->R=R;
    obj->x_mid=obj->x_last=obj->input;
    obj->filter=(double (*)(void *,double))xp_kalman_filter;
    obj->filter_s=(double (*)(void *))xp_kalman_filter_s;
    obj->init=1;
    return obj;
}


/*
kalman filter with data
1介滤波
param:
    obj:kalman object
    data:filter data
return:滤波得到的最新值
*/
double xp_kalman_filter(KALMAN_class *obj,double data){
    obj->input=data;
    obj->x_mid=obj->x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
    obj->p_mid=obj->p_last+obj->Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
    obj->kg=obj->p_mid/(obj->p_mid+obj->R); //kg为kalman filter，R为噪声
    obj->x_now=obj->x_mid+obj->kg*(obj->input-obj->x_mid);//估计出的最优值
    obj->p_now=(1-obj->kg)*obj->p_mid;//最优值对应的covariance
    obj->p_last =obj->p_now; //更新covariance值
    obj->x_last =obj->x_now; //更新系统状态值
    return obj->x_now;
}

/*
kalman filter use input
1介滤波
param:
    obj:kalman object
return:滤波得到的最新值
*/
double xp_kalman_filter_s(KALMAN_class *obj){
    obj->x_mid=obj->x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
    obj->p_mid=obj->p_last+obj->Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
    obj->kg=obj->p_mid/(obj->p_mid+obj->R); //kg为kalman filter，R为噪声
    obj->x_now=obj->x_mid+obj->kg*(obj->input-obj->x_mid);//估计出的最优值
    obj->p_now=(1-obj->kg)*obj->p_mid;//最优值对应的covariance
    obj->p_last =obj->p_now; //更新covariance值
    obj->x_last =obj->x_now; //更新系统状态值
    return obj->x_now;
}






//end of the file
